#include <mrpt/core.h>

#include "ParticleVehicleData.h"
#include "ParticleDataPredictor.h"
#include "ImageLandmarksVehicleWeightCalculator.h"

using namespace mrpt;
using namespace mrpt::bayes;
using namespace mrpt::slam;


class CBearingOnlyParticleFilter : public mrpt::bayes::CParticleFilterCapable, public mrpt::bayes::CParticleFilterData<CParticleVehicleData>
{
	// This uses CParticleFilterData to implement some methods required for CParticleFilterCapable:
	IMPLEMENT_PARTICLE_FILTER_CAPABLE(CParticleVehicleData);

public:
	void setInitialValues(const CParticleVehicleData& initialData) { this->initialVehicleData = initialData; }
	void setParticleDataPredictor(CParticleDataPredictor<CParticleVehicleData>* pPredictor);
	void setParticleDataWeightCalculator(CImageLandmarksVehicleWeightCalculator* pWeightCalculator);

public:

	 /** Update the m_particles, predicting the posterior of robot pose and map after a movement command.
	  *  This method has additional configuration parameters in "options".
	  *  Performs the update stage of the RBPF, using the sensed Sensorial Frame:
	  *
	  *   \param action This is a pointer to CActionCollection, containing the pose change the robot has been commanded.
	  *   \param observation This must be a pointer to a CSensoryFrame object, with robot sensed observations.
	  *
	  * \sa options
	  */
	void  prediction_and_update_pfStandardProposal(
		const mrpt::slam::CActionCollection	* action,
		const mrpt::slam::CSensoryFrame		* observation,
		const bayes::CParticleFilter::TParticleFilterOptions &PF_options );


	void initializeParticles(size_t  numParticles);

	/** Computes the average velocity & position
   	  */
	void getMean( float &x, float &y, float& w, float &vx, float &vy, float& vw );

private:

private:
	CParticleVehicleData initialVehicleData;
	CParticleDataPredictor<CParticleVehicleData>* pParticleDataPredictor;
	CImageLandmarksVehicleWeightCalculator* pWeightCalculator;
};

